#include "slam_robot/serial_bridge.hpp"
#include "slam_robot/imu_publisher.hpp"          
#include "slam_robot/odometry_publisher.hpp"     
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

class MainNode : public rclcpp::Node {
public:
    MainNode() : Node("slam_robot_main") {
        // 参数配置
        declare_parameter("serial_port", "/dev/ttyUSB0");
        declare_parameter("baud_rate", 115200);
        declare_parameter("max_linear_speed", 0.3);  // m/s (300mm/s)
        declare_parameter("max_angular_speed", 1.57); // rad/s

        std::string port = get_parameter("serial_port").as_string();
        int baudrate = get_parameter("baud_rate").as_int();

        // 初始化串口
        serial_ = std::make_shared<SerialBridge>(port, baudrate);
        
        // 创建静态TF广播器
        static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
        
        // 设置静态TF变换
        setupStaticTransforms();
        
        // 创建组件（传入父节点this）
        imu_publisher_ = std::make_shared<ImuPublisher>(this, serial_);
        odom_publisher_ = std::make_shared<OdometryPublisher>(this, serial_);

        // 速度命令订阅
        cmd_vel_sub_ = create_subscription<geometry_msgs::msg::Twist>(
            "cmd_vel", 10,
            [this](const geometry_msgs::msg::Twist::SharedPtr msg) {
                // 获取速度限制参数
                double max_linear = get_parameter("max_linear_speed").as_double();
                double max_angular = get_parameter("max_angular_speed").as_double();
                
                // 限幅处理
                double vx = std::clamp(msg->linear.x, -max_linear, max_linear);
                double vy = std::clamp(msg->linear.y, -max_linear, max_linear);
                double vz = std::clamp(msg->angular.z, -max_angular, max_angular);
                
                serial_->setSpeedCommand(
                    static_cast<int16_t>(vx * 1000),  // m/s → mm/s
                    static_cast<int16_t>(vy * 1000),
                    static_cast<int16_t>(vz)          // rad/s不变
                );
            });
    }

private:
    void setupStaticTransforms() {
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = now();
        transform.header.frame_id = "base_link";
        transform.child_frame_id = "imu_link";
        
        transform.transform.translation.x = 0.05;
        transform.transform.translation.y = 0.0;
        transform.transform.translation.z = 0.1;
        
        tf2::Quaternion q;
        q.setRPY(0, 0, 0);
        transform.transform.rotation.x = q.x();
        transform.transform.rotation.y = q.y();
        transform.transform.rotation.z = q.z();
        transform.transform.rotation.w = q.w();
        
        static_tf_broadcaster_->sendTransform(transform);
    }

    std::shared_ptr<SerialBridge> serial_;
    std::shared_ptr<ImuPublisher> imu_publisher_;
    std::shared_ptr<OdometryPublisher> odom_publisher_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<MainNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}